/*
 * PWM2ELRS - Arduino Mega2560连接8通道PWM接收机，将PWM信号转换为CRSF协议输出
 * 
 * 本程序用于Arduino Mega2560开发板，连接标准PWM接收机，
 * 将接收机输出的PWM信号读取后转换为CRSF协议数据输出到指定引脚
 * 
 * 硬件连接：
 * PWM接收机 -> Arduino Mega2560
 * 通道1    -> Pin 2 (中断0)
 * 通道2    -> Pin 3 (中断1)
 * 通道3    -> Pin 21 (中断2)
 * 通道4    -> Pin 20 (中断3)
 * 通道5    -> Pin 19 (中断4)
 * 通道6    -> Pin 18 (中断5)
 * 通道7    -> Pin 2 (模拟读取)
 * 通道8    -> Pin 3 (模拟读取)
 * 
 * CRSF输出：
 * Arduino Mega2560 -> ELRS接收机
 * TX2 (Pin 16)    -> RX
 */

// CRSF协议相关定义
#define CRSF_BAUDRATE           420000 // ELRS使用的CRSF波特率
#define CRSF_FRAME_SIZE_MAX     64
#define CRSF_HEADER_ORIGIN_MASK 0b11000000
#define CRSF_HEADER_ORIGIN_CRSF 0b00000000
#define CRSF_HEADER_TYPE_MASK   0b00111111
#define CRSF_FRAME_START_BYTE   0xC8
#define CRSF_ADDRESS_FLIGHT_CONTROLLER 0xC8
#define CRSF_ADDRESS_TRANSMITTER 0xEA
#define CRSF_TYPE_RC_CHANNELS   0x16

// PWM信号读取相关定义
#define PWM_CHANNELS 8
#define PWM_MIN 1000 // 最小PWM值 (微秒)
#define PWM_MAX 2000 // 最大PWM值 (微秒)
#define PWM_TIMEOUT 100000 // 100ms超时

// 通道数据结构 (11位通道值)
struct channelData_t {
  unsigned int ch0 : 11;
  unsigned int ch1 : 11;
  unsigned int ch2 : 11;
  unsigned int ch3 : 11;
  unsigned int ch4 : 11;
  unsigned int ch5 : 11;
  unsigned int ch6 : 11;
  unsigned int ch7 : 11;
  unsigned int ch8 : 11;
  unsigned int ch9 : 11;
  unsigned int ch10 : 11;
  unsigned int ch11 : 11;
  unsigned int ch12 : 11;
  unsigned int ch13 : 11;
  unsigned int ch14 : 11;
  unsigned int ch15 : 11;
} __attribute__((packed));

// 全局变量
volatile unsigned long pwmRiseTime[PWM_CHANNELS]; // PWM上升沿时间
volatile unsigned long pwmFallTime[PWM_CHANNELS]; // PWM下降沿时间
volatile unsigned int pwmValues[PWM_CHANNELS];    // PWM值 (微秒)
volatile bool pwmUpdated[PWM_CHANNELS];           // PWM值是否已更新

// 通道值映射
unsigned int channelValues[16] = {992, 992, 992, 992, 992, 992, 992, 992, 992, 992, 992, 992, 992, 992, 992, 992};

// CRSF发送相关变量
uint8_t crsfPacket[CRSF_FRAME_SIZE_MAX];
unsigned long lastCrsfSendTime = 0;
const unsigned long crsfSendInterval = 5; // 5ms发送一次 (200Hz)

// CRC8计算表
const uint8_t crc8_dvb_s2_table[256] = {
  0x00, 0xD5, 0x7F, 0xAA, 0xFE, 0x2B, 0x81, 0x54, 0x29, 0xFC, 0x56, 0x83, 0xD7, 0x02, 0xA8, 0x7D,
  0x52, 0x87, 0x2D, 0xF8, 0xAC, 0x79, 0xD3, 0x06, 0x7B, 0xAE, 0x04, 0xD1, 0x85, 0x50, 0xFA, 0x2F,
  0xA4, 0x71, 0xDB, 0x0E, 0x5A, 0x8F, 0x25, 0xF0, 0x8D, 0x58, 0xF2, 0x27, 0x73, 0xA6, 0x0C, 0xD9,
  0xF6, 0x23, 0x89, 0x5C, 0x08, 0xDD, 0x77, 0xA2, 0xDF, 0x0A, 0xA0, 0x75, 0x21, 0xF4, 0x5E, 0x8B,
  0x9D, 0x48, 0xE2, 0x37, 0x63, 0xB6, 0x1C, 0xC9, 0xB4, 0x61, 0xCB, 0x1E, 0x4A, 0x9F, 0x35, 0xE0,
  0xCF, 0x1A, 0xB0, 0x65, 0x31, 0xE4, 0x4E, 0x9B, 0xE6, 0x33, 0x99, 0x4C, 0x18, 0xCD, 0x67, 0xB2,
  0x39, 0xEC, 0x46, 0x93, 0xC7, 0x12, 0xB8, 0x6D, 0x10, 0xC5, 0x6F, 0xBA, 0xEE, 0x3B, 0x91, 0x44,
  0x6B, 0xBE, 0x14, 0xC1, 0x95, 0x40, 0xEA, 0x3F, 0x42, 0x97, 0x3D, 0xE8, 0xBC, 0x69, 0xC3, 0x16,
  0xEF, 0x3A, 0x90, 0x45, 0x11, 0xC4, 0x6E, 0xBB, 0xC6, 0x13, 0xB9, 0x6C, 0x38, 0xED, 0x47, 0x92,
  0xBD, 0x68, 0xC2, 0x17, 0x43, 0x96, 0x3C, 0xE9, 0x94, 0x41, 0xEB, 0x3E, 0x6A, 0xBF, 0x15, 0xC0,
  0x4B, 0x9E, 0x34, 0xE1, 0xB5, 0x60, 0xCA, 0x1F, 0x62, 0xB7, 0x1D, 0xC8, 0x9C, 0x49, 0xE3, 0x36,
  0x19, 0xCC, 0x66, 0xB3, 0xE7, 0x32, 0x98, 0x4D, 0x30, 0xE5, 0x4F, 0x9A, 0xCE, 0x1B, 0xB1, 0x64,
  0x72, 0xA7, 0x0D, 0xD8, 0x8C, 0x59, 0xF3, 0x26, 0x5B, 0x8E, 0x24, 0xF1, 0xA5, 0x70, 0xDA, 0x0F,
  0x20, 0xF5, 0x5F, 0x8A, 0xDE, 0x0B, 0xA1, 0x74, 0x09, 0xDC, 0x76, 0xA3, 0xF7, 0x22, 0x88, 0x5D,
  0xD6, 0x03, 0xA9, 0x7C, 0x28, 0xFD, 0x57, 0x82, 0xFF, 0x2A, 0x80, 0x55, 0x01, 0xD4, 0x7E, 0xAB,
  0x84, 0x51, 0xFB, 0x2E, 0x7A, 0xAF, 0x05, 0xD0, 0xAD, 0x78, 0xD2, 0x07, 0x53, 0x86, 0x2C, 0xF9
};

// 计算CRC8校验
uint8_t crc8_calc(uint8_t *data, uint8_t len) {
  uint8_t crc = 0;
  for (uint8_t i = 0; i < len; i++) {
    crc = crc8_dvb_s2_table[crc ^ data[i]];
  }
  return crc;
}

// 将标准PWM值(1000-2000)映射到CRSF的11位通道值(172-1811)
unsigned int mapToCrsfValue(unsigned int pwmValue) {
  // 限制PWM值在有效范围内
  pwmValue = constrain(pwmValue, PWM_MIN, PWM_MAX);
  // 映射到CRSF值范围
  return map(pwmValue, PWM_MIN, PWM_MAX, 172, 1811);
}

// PWM中断处理函数
void pwmRising0() { pwmRiseTime[0] = micros(); attachInterrupt(0, pwmFalling0, FALLING); }
void pwmFalling0() { pwmFallTime[0] = micros(); pwmValues[0] = pwmFallTime[0] - pwmRiseTime[0]; pwmUpdated[0] = true; attachInterrupt(0, pwmRising0, RISING); }

void pwmRising1() { pwmRiseTime[1] = micros(); attachInterrupt(1, pwmFalling1, FALLING); }
void pwmFalling1() { pwmFallTime[1] = micros(); pwmValues[1] = pwmFallTime[1] - pwmRiseTime[1]; pwmUpdated[1] = true; attachInterrupt(1, pwmRising1, RISING); }

void pwmRising2() { pwmRiseTime[2] = micros(); attachInterrupt(2, pwmFalling2, FALLING); }
void pwmFalling2() { pwmFallTime[2] = micros(); pwmValues[2] = pwmFallTime[2] - pwmRiseTime[2]; pwmUpdated[2] = true; attachInterrupt(2, pwmRising2, RISING); }

void pwmRising3() { pwmRiseTime[3] = micros(); attachInterrupt(3, pwmFalling3, FALLING); }
void pwmFalling3() { pwmFallTime[3] = micros(); pwmValues[3] = pwmFallTime[3] - pwmRiseTime[3]; pwmUpdated[3] = true; attachInterrupt(3, pwmRising3, RISING); }

void pwmRising4() { pwmRiseTime[4] = micros(); attachInterrupt(4, pwmFalling4, FALLING); }
void pwmFalling4() { pwmFallTime[4] = micros(); pwmValues[4] = pwmFallTime[4] - pwmRiseTime[4]; pwmUpdated[4] = true; attachInterrupt(4, pwmRising4, RISING); }

void pwmRising5() { pwmRiseTime[5] = micros(); attachInterrupt(5, pwmFalling5, FALLING); }
void pwmFalling5() { pwmFallTime[5] = micros(); pwmValues[5] = pwmFallTime[5] - pwmRiseTime[5]; pwmUpdated[5] = true; attachInterrupt(5, pwmRising5, RISING); }

// 创建CRSF通道数据包
void createCrsfChannelsPacket() {
  // 更新通道值
  for (int i = 0; i < PWM_CHANNELS; i++) {
    if (pwmUpdated[i]) {
      channelValues[i] = mapToCrsfValue(pwmValues[i]);
      pwmUpdated[i] = false;
    }
  }
  
  // 创建通道数据结构
  channelData_t channelData;
  channelData.ch0 = channelValues[0];
  channelData.ch1 = channelValues[1];
  channelData.ch2 = channelValues[2];
  channelData.ch3 = channelValues[3];
  channelData.ch4 = channelValues[4];
  channelData.ch5 = channelValues[5];
  channelData.ch6 = channelValues[6];
  channelData.ch7 = channelValues[7];
  channelData.ch8 = channelValues[8];
  channelData.ch9 = channelValues[9];
  channelData.ch10 = channelValues[10];
  channelData.ch11 = channelValues[11];
  channelData.ch12 = channelValues[12];
  channelData.ch13 = channelValues[13];
  channelData.ch14 = channelValues[14];
  channelData.ch15 = channelValues[15];
  
  // 构建CRSF数据包
  uint8_t len = 24; // RC通道数据包长度 (地址1 + 长度1 + 类型1 + 通道数据22 + CRC1)
  
  crsfPacket[0] = CRSF_ADDRESS_FLIGHT_CONTROLLER; // 目标地址
  crsfPacket[1] = len - 2; // 长度字段不包括地址和自身
  crsfPacket[2] = CRSF_TYPE_RC_CHANNELS; // 类型：RC通道
  
  // 复制通道数据
  memcpy(&crsfPacket[3], &channelData, 22);
  
  // 计算并添加CRC
  crsfPacket[len - 1] = crc8_calc(crsfPacket, len - 1);
}

// 发送CRSF数据包
void sendCrsfPacket() {
  createCrsfChannelsPacket();
  
  // 通过Serial2发送数据包
  Serial2.write(crsfPacket, 26); // 24字节的RC通道数据包 + 2字节的帧头
}

void setup() {
  // 初始化串口0用于调试输出
  Serial.begin(115200);
  Serial.println("PWM2ELRS - PWM to CRSF Converter");
  Serial.println("For Arduino Mega2560");
  
  // 初始化串口2用于CRSF输出
  Serial2.begin(CRSF_BAUDRATE);
  
  // 初始化PWM输入引脚和中断
  pinMode(2, INPUT); // 通道1
  pinMode(3, INPUT); // 通道2
  pinMode(21, INPUT); // 通道3
  pinMode(20, INPUT); // 通道4
  pinMode(19, INPUT); // 通道5
  pinMode(18, INPUT); // 通道6
  pinMode(A0, INPUT); // 通道7 (模拟)
  pinMode(A1, INPUT); // 通道8 (模拟)
  
  // 设置中断
  attachInterrupt(0, pwmRising0, RISING); // 通道1 (Pin 2)
  attachInterrupt(1, pwmRising1, RISING); // 通道2 (Pin 3)
  attachInterrupt(2, pwmRising2, RISING); // 通道3 (Pin 21)
  attachInterrupt(3, pwmRising3, RISING); // 通道4 (Pin 20)
  attachInterrupt(4, pwmRising4, RISING); // 通道5 (Pin 19)
  attachInterrupt(5, pwmRising5, RISING); // 通道6 (Pin 18)
  
  // 初始化通道值为中间值
  for (int i = 0; i < PWM_CHANNELS; i++) {
    pwmValues[i] = 1500;
    pwmUpdated[i] = true;
  }
  
  Serial.println("Initialization complete. Reading PWM signals...");
}

void loop() {
  // 读取模拟通道 (通道7和通道8)
  // 注意：这种方式读取PWM不够精确，仅作为示例
  // 实际应用中可能需要更精确的方法或使用更多的中断引脚
  int analogValue = analogRead(A0); // 0-1023
  pwmValues[6] = map(analogValue, 0, 1023, PWM_MIN, PWM_MAX);
  pwmUpdated[6] = true;
  
  analogValue = analogRead(A1); // 0-1023
  pwmValues[7] = map(analogValue, 0, 1023, PWM_MIN, PWM_MAX);
  pwmUpdated[7] = true;
  
  // 检查PWM信号是否超时
  unsigned long currentTime = micros();
  for (int i = 0; i < 6; i++) { // 只检查中断通道
    if (currentTime - pwmFallTime[i] > PWM_TIMEOUT) {
      // 信号丢失，设置为中间值
      pwmValues[i] = 1500;
      pwmUpdated[i] = true;
    }
  }
  
  // 定时发送CRSF数据包
  if (millis() - lastCrsfSendTime >= crsfSendInterval) {
    lastCrsfSendTime = millis();
    sendCrsfPacket();
  }
  
  // 每秒打印一次通道值（用于调试）
  static unsigned long lastPrintTime = 0;
  if (millis() - lastPrintTime > 1000) {
    lastPrintTime = millis();
    
    Serial.print("PWM Values: ");
    for (int i = 0; i < PWM_CHANNELS; i++) {
      Serial.print(pwmValues[i]);
      Serial.print(" ");
    }
    Serial.println();
    
    Serial.print("CRSF Values: ");
    for (int i = 0; i < 8; i++) {
      Serial.print(channelValues[i]);
      Serial.print(" ");
    }
    Serial.println();
  }
}